#ifndef POSE_UPDATER_H
#define POSE_UPDATER_H

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Quaternion.h"
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/LinkStates.h"
#include "std_msgs/String.h"
#include <string>
#include <vector>
#include "tf/tf.h"
#include <math.h>

class PoseUpdater
{
public:
	PoseUpdater();
	PoseUpdater(ros::NodeHandle *ptr_n){
		this->pn = ptr_n;
		this->rear_pose_pub = pn->advertise<geometry_msgs::PoseStamped>("/smart/rear_pose", 100);
		this->center_pose_pub = pn->advertise<geometry_msgs::PoseStamped>("/smart/center_pose", 100);
		this->vel_pub = pn->advertise<geometry_msgs::TwistStamped>("/smart/velocity", 100);
		this->gazebo_pose_sub = pn->subscribe("/gazebo/model_states", 100, &PoseUpdater::pose_cb, this);
	}
	virtual ~PoseUpdater();

private:
	ros::NodeHandle *pn;
	ros::Publisher rear_pose_pub;
	ros::Publisher center_pose_pub;
	ros::Publisher vel_pub;
	ros::Subscriber gazebo_pose_sub;
	
public:
	void pose_cb(const gazebo_msgs::ModelStates &msg);
};


#endif //POSE_UPDATER_H